/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package deatax.math;

/**
 *
 * @author pson
 */
public class Matrix3 {
    
    public float v[];
    
    public Matrix3() {
        v = new float[] {
            1, 0, 0,
            0, 1, 0,
            0, 0, 1
        };
    }
    

    public Matrix3(float[] val) {
        this.v = val;
    }
    
    public Matrix3(Matrix3 m) {
        this(m.v);
    }
    
    public Matrix3(Matrix4 m) {
        float[] mv = m.v;
        this.v = new float[] {
            mv[0], mv[1], mv[2],
            mv[4], mv[5], mv[6],
            mv[8], mv[9], mv[10]
        };
    }
    
    public boolean equals(Matrix3 m) {
        float[] mv = m.v;
        return v[0] == mv[0] && v[1] == mv[1] && v[2] == mv[2] &&
               v[3] == mv[3] && v[4] == mv[4] && v[5] == mv[5] &&
               v[6] == mv[6] && v[7] == mv[7] && v[8] == mv[8];
    }
    
    public Matrix3 multiply(Matrix3 m) {
        float[] mv = m.v;
        return new Matrix3(new float[]{
            mv[0] * v[0] + mv[3] * v[1] + mv[6] * v[2],
            mv[1] * v[0] + mv[4] * v[1] + mv[7] * v[2],
            mv[2] * v[0] + mv[5] * v[1] + mv[8] * v[2],
            mv[0] * v[3] + mv[3] * v[4] + mv[6] * v[5],
            mv[1] * v[3] + mv[4] * v[4] + mv[7] * v[5],
            mv[2] * v[3] + mv[5] * v[4] + mv[8] * v[5],
            mv[0] * v[6] + mv[3] * v[7] + mv[6] * v[8],
            mv[1] * v[6] + mv[4] * v[7] + mv[7] * v[8],
            mv[2] * v[6] + mv[5] * v[7] + mv[8] * v[8]
        });
    }
    
    public Vector3 multiply(Vector3 vec) {
        return new Vector3(
                v[0] * vec.x + v[1] * vec.y + v[2] * vec.z,
                v[3] * vec.x + v[4] * vec.y + v[5] * vec.z,
                v[6] * vec.x + v[7] * vec.y + v[8] * vec.z);
    }
    
    public Matrix3 transposed() {
        return new Matrix3(new float[] {
            v[0], v[3], v[6],
            v[1], v[4], v[7],
            v[2], v[5], v[8]
        });
    }
    
    public float determinate() {
        return v[0]*v[4]*v[8] - v[0]*v[5]*v[7] + v[1]*v[5]*v[6] - v[1]*v[3]*v[8] + v[2]*v[3]*v[7] - v[2]*v[4]*v[6];
    }
    
    public Matrix3 inversed() {
        float d = determinate();
        if (d == 0.0f) {
            System.err.println("Cannot invert matrix!");
            return null;
        }
        
        d = 1.0f / d;
        
        return new Matrix3(new float[]{
            d  * (v[4] * v[8] - v[5] * v[7]),
            -d * (v[1] * v[8] - v[2] * v[7]),
            d  * (v[1] * v[5] - v[2] * v[4]),
            -d * (v[3] * v[8] - v[5] * v[6]),
            d  * (v[0] * v[8] - v[2] * v[6]),
            -d * (v[0] * v[5] - v[2] * v[3]),
            d  * (v[3] * v[7] - v[4] * v[6]),
            -d * (v[0] * v[7] - v[1] * v[6]),
            d  * (v[0] * v[4] - v[1] * v[3])
        });
    }

    public static Matrix3 rotationZ(float a) {
        float s = (float)Math.sin(a);
        float c = (float)Math.cos(a);
        
        return new Matrix3(new float[] {
            c,    -s,   0.0f,
            s,    c,    0.0f,
            0.0f, 0.0f, 1.0f
        });
    }
    
    public static Matrix3 rotation(float a, Vector3 axis) {
        float c = (float)Math.cos(a);
        float s = (float)Math.sin(a);
        float t = 1 - c;
        
        float x = axis.x;
        float y = axis.y;
        float z = axis.z;
        
        return new Matrix3(new float[] {
            t*x*x+c,   t*x*y-s*z, t*x*z+s*y,
            t*x*y+s*z, t*y*y+c,   t*y*z-s*x,
            t*x*z-s*y, t*y*z+s*x, t*z*z+c
        });
    }
    
    public static Matrix3 translation(Vector2 vec) {
        Matrix3 m = new Matrix3();
        m.v[2] = vec.x;
        m.v[5] = vec.y;
        return m;
    }
    
    public static Matrix3 fromToMatrix(Vector3 from, Vector3 to) {
        final float e = from.dot(to);
        final float f = Math.abs(e);
        if (f > 1.0 - 0.00001f) {
            Vector3 vx = new Vector3(Math.abs(from.x), Math.abs(from.y), Math.abs(from.z));
            if (vx.x < vx.y) {
                if (vx.x < vx.z) {
                    vx.x = 1.0f;
                    vx.y = 0.0f;
                    vx.z = 0.0f;
                } else {
                    vx.x = 0.0f;
                    vx.y = 0.0f;
                    vx.z = 1.0f;
                }
            } else {
                if (vx.y < vx.z) {
                    vx.x = 0.0f;
                    vx.y = 1.0f;
                    vx.z = 0.0f;
                } else {
                    vx.x = 0.0f;
                    vx.y = 0.0f;
                    vx.z = 1.0f;
                }
            }
            Vector3 vu = new Vector3(vx.x - from.x, vx.y - from.y, vx.z - from.z);
            Vector3 vv = new Vector3(vx.x - to.x, vx.y - to.y, vx.z - to.z);
            
            final float c1 = 2.0f / (vu.dot(vu));
            final float c2 = 2.0f / (vv.dot(vv));
            final float c3 = c1 * c2 * (vu.dot(vv));
            
            Matrix3 m = new Matrix3();
            
            for (int i = 0; i < 3; ++i) {
                for (int j = 0; j < 3; ++j) {
                    float[] ua = vu.toArray();
                    float[] va = vv.toArray();
                    
                    m.v[i*3+j] = - c1 * ua[i] * ua[j] - c2 * va[i] * va[j] + c3 * va[i] * ua[j];
                }
                m.v[i*3+i] = 1.0f;
            }
            return m;
            
        } else {
            Vector3 vv = from.cross(to);
            final float h = 1.0f / (1.0f + e);
            final float hvx = h * vv.x;
            final float hvz = h * vv.z;
            final float hvxy = hvx * vv.y;
            final float hvxz = hvx * vv.z;
            final float hvyz = hvz * vv.y;
            
            return new Matrix3(new float[] {
                e + hvx * vv.x, hvxy - vv.z, hvxz + vv.y,
                hvxy + vv.z, e + h * vv.y * vv.y, hvyz - vv.x,
                hvxz - vv.y, hvyz + vv.x, e + hvz * vv.z
            });
        }
    }
}
